﻿using System;
using System.Drawing;
using System.Drawing.Imaging;
using System.Runtime.InteropServices;
using System.Threading;

namespace WinwiseARDrone.API
{
    /// <summary>
    /// Classe statique pour contrôler le drone
    /// </summary>
    public static class DroneController
    {
        #region Interop Win32

        [DllImport("Win32Client.dll")]
        internal static extern int detect_drone_connection();

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.StdCall)]
        internal static extern int init_drone([MarshalAs(UnmanagedType.Struct)] Input_Device input, [MarshalAs(UnmanagedType.FunctionPtr)]DroneNavigationData nav, [MarshalAs(UnmanagedType.FunctionPtr)] VideoStream video);

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.StdCall)]
        internal static extern int update_drone();

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.ThisCall)]
        internal static extern int shutdown_drone();

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.StdCall)]
        internal static extern void ardrone_at_set_progress_cmd(int enable, float phi, float theta, float gaz, float yaw);

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.StdCall)]
        internal static extern void ardrone_at_set_flat_trim();

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.StdCall)]
        internal static extern int ardrone_tool_set_ui_pad_select(Int32 value);

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.StdCall)]
        internal static extern int ardrone_tool_set_ui_pad_start(Int32 value);

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.StdCall)]
        internal static extern void ardrone_at_zap(ZapVideoChannel channel);

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.StdCall)]
        internal static extern IntPtr ctrl_state_str(UInt32 ctrl_state);

        [DllImport("Win32Client.dll", CharSet = CharSet.Auto, PreserveSig = true, CallingConvention = CallingConvention.StdCall)]
        internal static extern void switch_video_channel(string channel);
        #endregion

        #region Constantes

        private const int DRONE_WIDTH = 640;
        private const int DRONE_HEIGHT = 480;

        #endregion

        #region Variables

        private static DroneNavigationData nav;
        private static VideoStream video;

        private static bool shutdown;

        #endregion

        #region Accesseurs

        /// <summary>
        /// Obtient ou définie les informations du drone
        /// </summary>
        public static HandleDroneInput DroneInput
        {
            get;
            set;
        }

        #endregion

        #region Evénements

        public static event EventHandler<NavDataArgs> NavigationData;
        public static event EventHandler<VideoStreamArgs> VideoStream;

        #endregion

        #region Constructeurs

        /// <summary>
        /// Constructeur statique
        /// </summary>
        static DroneController()
        {
            DroneController.DroneInput = new HandleDroneInput();
        }

        #endregion

        private static void DroneLoop()
        {
            DroneController.ardrone_at_set_flat_trim();

            while (!shutdown)
            {
                DroneController.update_drone();
            }

            // La pluspart du temps, l'API Parrot crash 
            // lors du shutdown, on évite juste que ca
            // pollue notre applicaiton

            try
            {
                DroneController.shutdown_drone();
            }
            catch
            {
            }
        }

        public static void Emergency()
        {
            DroneInput.Emergency = true;
        }

        public static int InitDrone()
        {
            nav = new DroneNavigationData(OnNavigationData);
            video = new VideoStream(OnVideoFrame);

            shutdown = false;

            int res = DroneController.detect_drone_connection();

            if (res == 0)
            {
                res = DroneController.init_drone(Input_Device.Input, nav, video);
            }

            if (res == 0)
            {
                Thread thread = new Thread(DroneLoop);
                thread.Start();
            }

            return res;
        }

        public static void Launch()
        {
            DroneInput.Launch = true;
        }

        public static void Land()
        {
            DroneInput.Land = true;
        }

        public static void Shutdown()
        {
            shutdown = true;
        }

        public static void SwitchVideo(ZapVideoChannel channel)
        {
            switch_video_channel(((int)channel).ToString());
        }

        public static void OnNavigationData(IntPtr nd)
        {
            navdata_demo_t nav;

            nav = (navdata_demo_t)Marshal.PtrToStructure(nd, typeof(navdata_demo_t));

            if (NavigationData != null)
            {
                NavigationData(null, new NavDataArgs()
                {
                    NavData = nav
                });
            }
        }

        public static void OnVideoFrame(int width, int height, IntPtr buffer)
        {
            byte[] rgb = new byte[DRONE_WIDTH * (DRONE_HEIGHT * 3)];
            Marshal.Copy(buffer, rgb, 0, rgb.Length);

            Bitmap bmp = new Bitmap(width, height, PixelFormat.Format24bppRgb);

            Rectangle rect = new Rectangle(0, 0, bmp.Width, bmp.Height);
            BitmapData bmpData = bmp.LockBits(rect, ImageLockMode.ReadWrite, bmp.PixelFormat);

            IntPtr ptr = bmpData.Scan0;

            for (int j = 0; j < height; j++)
            {
                int destLine = j * width * 3;
                int srcLine = j * DRONE_WIDTH * 3;

                for (int i = 0; i < width * 3; i += 3)
                {
                    Marshal.WriteByte(ptr, destLine + i, rgb[srcLine + i + 2]);
                    Marshal.WriteByte(ptr, destLine + i + 1, rgb[srcLine + i + 1]);
                    Marshal.WriteByte(ptr, destLine + i + 2, rgb[srcLine + i]);
                }
            }

            bmp.UnlockBits(bmpData);

            if (VideoStream != null)
            {
                VideoStream(null, new VideoStreamArgs()
                {
                    Image = bmp
                });
            }
        }
    }
}
